#include "msp.h"
#include "controller.h"
#include "motor.h"

typedef struct {
    int8_t left;
    int8_t right;
    int8_t vert;
} motor_ctl_args_t;


// STATIC FUNCTION DECLARATIONS
static void motor_ctl_fn(motor_ctl_args_t *mca);


void initialize_controller (void) {
    // DUMMY

    return;
}

void controller_tick (uint8_t *raw_command) {
    // Break out the opcode and args
    opcode_t opcode = (opcode_t)raw_command[0];
    uint8_t *args = raw_command + 1;
    switch (opcode) {
        
    case MOTOR_CTL:
        motor_ctl_fn((motor_ctl_args_t *)args);
        break;
        
    case DIE:
        {
            motor_ctl_args_t mca = {0,0,0};
            motor_ctl_fn(&mca);
        }
        break;
        
    default:
        break;
    }
    return;
}

static void motor_ctl_fn(motor_ctl_args_t *mca) {
    set_motor_speed(mca->left, mca->right, mca->vert);
    return;
}
